#ifndef PNP_H_
#define PNP_H_

#include <iostream>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <sophus/se3.hpp>

using namespace std;
using namespace cv;

void find_feature_matches(const Mat &img_1, const Mat &img_2,
                          vector<KeyPoint> &keypoints_1,
                          vector<KeyPoint> &keypoints_2,
                          vector<DMatch> &matches);

Point2d pixel2cam(const Point2d &p, const Mat &K);

typedef vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> VecVector2d;
typedef vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> VecVector3d;

void bundle_adjustment_gauss_newton(const VecVector3d &points_3d,
                                    const VecVector2d &points_2d,
                                    const Mat &K,
                                    Sophus::SE3d &pose);

void bundle_adjustment_g2o(const VecVector3d &points_3d,
                           const VecVector2d &points_2d,
                           const Mat &K,
                           Sophus::SE3d &pose);

#endif
